/*
 * circle.c
 *
 *  Created on: 2023骞 3鏈 31鏃 
 *      Author: 0
 */

#include "zf_common_headfile.h"

// 圆环状态标志位
enum circle_type_e circle_type = CIRCLE_NONE;

// 圆环状态标志位名称
const char *circle_type_name[CIRCLE_NUM] = {
        "CIRCLE_NONE                ",
        "CIRCLE_LEFT_BEGIN          ", "CIRCLE_RIGHT_BEGIN          ",
        "CIRCLE_lost_line_LEFT      ", "CIRCLE_lost_line_RIGHT      ",
        "CIRCLE_find_line_LEFT      ", "CIRCLE_find_line_RIGHT      ",
        "CIRCLE_lost_line_again_LEFT", "CIRCLE_lost_line_again_RIGHT",
        "CIRCLE_LEFT_IN             ", "CIRCLE_RIGHT_IN             ",
        "CIRCLE_LEFT_RUNNING        ", "CIRCLE_RIGHT_RUNNING        ",
        "CIRCLE_LEFT_OUT            ", "CIRCLE_RIGHT_OUT            ",
        "CIRCLE_LEFT_END            ", "CIRCLE_RIGHT_END            "
};

// 左右圆环边线拐角在原图坐标
int Lpt1_c_x = 0, Lpt1_c_y = 0;
// 临时变量(补线使用,表示最后一个元素所在下标)
int temp_line = 0, temp_1 = 0, temp_2 = 0;

// 出圆环时圆环外线拐点前边线斜率(补线使用)
float Lpt_k = 0.0f, Lpt_b = 0.0f;

int cipts0[POINTS_MAX_LEN][2]; // 在原图上圆环左线的坐标
int cipts1[POINTS_MAX_LEN][2]; // 在原图上圆环右线的坐标
int cipts0_num = 0, cipts1_num = 0;

float crpts0[POINTS_MAX_LEN][2];  // 逆透视后的左线坐标
float crpts1[POINTS_MAX_LEN][2];  // 逆透视后的右线坐标
int crpts0_num = 0, crpts1_num = 0;

float crpts0b[POINTS_MAX_LEN][2];  // 滤波后的逆透视左线坐标
float crpts1b[POINTS_MAX_LEN][2];  // 滤波后的逆透视右线坐标
int crpts0b_num = 0, crpts1b_num = 0;

float crpts0s[POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视左线坐标
float crpts1s[POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视右线坐标
int crpts0s_num = 0, crpts1s_num = 0;

float crpts0a[POINTS_MAX_LEN];  // 左线局部角变化率,弧度
float crpts1a[POINTS_MAX_LEN];  // 右线局部角变化率,弧度
int crpts0a_num = 0, crpts1a_num = 0;

float crpts0an[POINTS_MAX_LEN];  // 左线角变化率非极大抑制
float crpts1an[POINTS_MAX_LEN];  // 右线角变化率非极大抑制
int crpts0an_num = 0, crpts1an_num = 0;

float crptsc0[POINTS_MAX_LEN][2];  // 左中线
float crptsc1[POINTS_MAX_LEN][2];  // 右中线
int crptsc0_num = 0, crptsc1_num = 0;

// 记录圆环外边线拐点坐标
int Lpt0_circle_id = 0, Lpt1_circle_id = 0;
// 找圆环外边线拐点
bool Lpt0_circle_found = false, Lpt1_circle_found = false;
// 左右圆环外线起始点(爬线使用)
int cx1 = 0, cy1 = 0, cx0 = 0, cy0 = 0;

// 检测前方是否有圆环
void check_circle (void)
{
    // 检测左圆环
    // 当前状态为前方无圆环,左边线向左拐,拐点距离车小于30cm,左边线无右拐,右边线无向左拐和向右拐的直线,无跳点
    if (JOIN_OTHER_STATE && Lpt0_found_left && Lpt0_rpts0s_id_left < (0.30 / sample_dist)
            && (!Lpt0_found_right) && (!Lpt1_found_left) && (!Lpt1_found_right) && is_straight1)
    {
        // 圆环在左侧,改变状态为准备进左侧圆环
        circle_type = CIRCLE_LEFT_BEGIN;
        // 要进圆环沿另一侧直线
        track_type = TRACK_RIGHT;
    }
    // 若当前状态为准备进左侧圆环
    if (circle_type == CIRCLE_LEFT_BEGIN)
    {
        // 长度小于15cm
        if (rpts0s_num < 0.15 / sample_dist)
        {
            // 改变状态为丢失左线
            circle_type = CIRCLE_check_lost_line_LEFT;
            // 左线丢失,循右线
            track_type = TRACK_RIGHT;
        }
        // 若右线为有左拐角和右拐角的非直线
        if ((Lpt1_found_left) && (Lpt1_found_right) && (!is_straight1) \
                || (Lpt0_found_left == false && rpts0s_num > 0.60 / sample_dist))
        {
            // 改变状态为无圆环
            circle_type = CIRCLE_NONE;
        }
    }
    // 若当前状态为丢左线,左线长度大于60cm
    if (circle_type == CIRCLE_check_lost_line_LEFT && rpts0s_num > 0.60 / sample_dist)
    {
        // 改变状态为找到左线
        circle_type = CIRCLE_check_find_line_LEFT;

        // 循右线
        track_type = TRACK_RIGHT;
    }
    // 若当前状态为找到左线,左线长度小于50cm,大于10cm
    if (circle_type == CIRCLE_check_find_line_LEFT && rpts0s_num > 0.10 / sample_dist && rpts0s_num < 0.50 / sample_dist)
    {
        // 改变状态为再次丢左线
        circle_type = CIRCLE_check_lost_line_again_LEFT;
        track_type = TRACK_LEFT;
    }

    // 检测右圆环
    // 当前状态为前方无圆环,右边线向右拐,拐点距离车小于30cm,右边线无左拐,左边线无向右拐和向左拐的直线,无跳点
    if (JOIN_OTHER_STATE && Lpt1_found_right && Lpt1_rpts1s_id_right < (0.30 / sample_dist)
            && (!Lpt1_found_left) && (!Lpt0_found_left) && (!Lpt0_found_right) && is_straight0)
    {
        // 圆环在右侧,改变状态为准备进右侧圆环
        circle_type = CIRCLE_RIGHT_BEGIN;
        // 要进圆环沿另一侧直线
        track_type = TRACK_LEFT;
    }
    // 若当前状态为准备进右侧圆环
    if (circle_type == CIRCLE_RIGHT_BEGIN)
    {
        // 长度小于15cm
        if (rpts1s_num < 0.15 / sample_dist)
        {
            // 改变状态为丢失右线
            circle_type = CIRCLE_check_lost_line_RIGHT;
            // 右线丢失,循左线
            track_type = TRACK_LEFT;
        }
        // 若左线为有右拐角和左拐角的非直线
        if ((Lpt0_found_left) && (Lpt0_found_right) && (!is_straight0) \
                || (Lpt1_found_right == false && rpts1s_num > 0.60 / sample_dist))
        {
            // 改变状态为无圆环
            circle_type = CIRCLE_NONE;
        }
    }
    // 若当前状态为丢右线,右线长度大于60cm
    if (circle_type == CIRCLE_check_lost_line_RIGHT && rpts1s_num > 0.60 / sample_dist)
    {
        // 改变状态为找到左线
        circle_type = CIRCLE_check_find_line_RIGHT;

        // 循右线
        track_type = TRACK_LEFT;
    }
    // 若当前状态为找到右线,右线长度小于50cm,大于10cm
    if (circle_type == CIRCLE_check_find_line_RIGHT && rpts1s_num > 0.10 / sample_dist && rpts1s_num < 0.50 / sample_dist)
    {
        // 改变状态为找到再次丢右线
        circle_type = CIRCLE_check_lost_line_again_RIGHT;
        track_type = TRACK_RIGHT;
    }
}

void run_circle (void)
{
    uint8 value = 0;

    if(circle_type == CIRCLE_check_lost_line_again_LEFT)
    {
        // 记录圆环内边线中最靠上的坐标值,为CIRCLE_IN爬线做起点
        int point_index = 0;
        for (int i = 1; i < ipts0_num; ++i)
        {
            if (ipts0[i][1] < ipts0[point_index][1])
                point_index = i;
        }
        cx0 = temp_1 = ipts0[point_index][0] + 15, cy0 = temp_2 = ipts0[point_index][1] - 10;

        track_type = TRACK_LEFT;
        if (rpts0s_num > 0.10 / sample_dist && rpts0s_num < 0.20 / sample_dist)
        {
            // 改变状态为进入左圆环
            circle_type = CIRCLE_LEFT_IN;
            track_type = TRACK_LEFT;
        }
    }
    else if (circle_type == CIRCLE_LEFT_IN)
    {
        // 当前圆环状态为进入左侧圆环
        cipts0_num = sizeof(cipts0) / sizeof(cipts0[0]);

        // 若圆环内边线足够长则再次查找最靠上点
        if (ipts0_num >= 0.06 / sample_dist && ipts0_num <= 0.30 / sample_dist)
        {
            int point_index = 0;
            for (int i = 1; i < ipts0_num; ++i)
            {
                if (ipts0[i][1] < ipts0[point_index][1])
                    point_index = i;
            }
            cx0 = temp_1 = ipts0[point_index][0] + 15, cy0 = temp_2 = ipts0[point_index][1] - 10;
        }
        else
        {
            // 不够长时使用前一次查找的坐标
            cx0 = temp_1;
            cy0 = temp_2;
        }
        // 向上找到圆环外边线
        for ( ; cy0 - 1 > block_size; cy0--)
        {
            if (AT_IMAGE(&img_raw, cx0, cy0 - 1) < thres)
                break;
        }

        // 爬圆环外线
        findline_lefthand_adaptive(&img_raw, block_size, thres, cx0, cy0, cipts0, &cipts0_num, 0);
        // 对圆环外线进行进行逆透视变换,将爬到的在原图的坐标转换为在逆透视图上的坐标
        if (cipts0_num != 0)
        {
            for (int i = 0; i < cipts0_num; i++)
            {
                value = ImagePerspective(cipts0[i][0], cipts0[i][1], &crpts0[i][0], &crpts0[i][1]);
                if (value == 1)
                    break;
                crpts0_num = i;
            }
        }
        else
            crpts0_num = 0;

        // 对外边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts0, crpts0_num, crpts0b, (int) round(line_blur_kernel));
        crpts0b_num = crpts0_num;

        // 对外边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts0s_num = sizeof(crpts0s) / sizeof(crpts0s[0]);
        resample_points2(crpts0b, crpts0b_num, crpts0s, &crpts0s_num, sample_dist * pixel_per_meter);

        // 计算外边线的局部角变化率
        local_angle_points(crpts0s, crpts0s_num, crpts0a, (int) round(angle_dist / sample_dist));
        crpts0a_num = crpts0s_num;

        cipts1_num = sizeof(cipts1) / sizeof(cipts1[0]);

        // 查找外边线上拐角
        find_circle_corners(1, crpts0a, crpts0a_num, &Lpt0_circle_id);

        // 将拐点转换为在原图坐标存入右边线的圆环x,y
        inv_Perspective(crpts0s[Lpt0_circle_id][0], crpts0s[Lpt0_circle_id][1], &Lpt1_c_x, &Lpt1_c_y);

        // 进行补线,右边线起始点补线至右边线圆环x,y,存入右线
        temp_line = ImageAddingLine(cipts1, ipts1[0][0], ipts1[0][1], Lpt1_c_x, Lpt1_c_y, 0, cipts1_num);

        // 将圆环外边线拐点前的线段逆透视放入圆环右线中
        for (int i = temp_line, j = Lpt0_circle_id; ; i++, j--)
        {
            if (j > 0 && i < cipts1_num)
            {
                inv_Perspective(crpts0s[j][0], crpts0s[j][1], &cipts1[i][0], &cipts1[i][1]);
            }
            else
            {
                cipts1_num = i;
                break;
            }
        }

        // 将在原图上的补出的线转换在逆透视图像上
        if (cipts1_num != 0)
        {
            for (int i = 0; i < cipts1_num; i++)
            {
                value = ImagePerspective(cipts1[i][0], cipts1[i][1], &crpts1[i][0], &crpts1[i][1]);
                if (value == 1)
                    break;
                crpts1_num = i;
            }
        }
        else
            crpts1_num = 0;

        // 对右边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts1, crpts1_num, crpts1b, (int) round(line_blur_kernel));
        crpts1b_num = crpts1_num;

        // 对右边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts1s_num = sizeof(crpts1s) / sizeof(crpts1s[0]);
        resample_points2(crpts1b, crpts1b_num, crpts1s, &crpts1s_num, sample_dist * pixel_per_meter);

        // 计算中线
        track_rightline(crpts1s, crpts1s_num, crptsc1, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
        crptsc1_num = crpts1s_num;
        // 循圆环右线
        track_type = TRACK_CIRCLE_RIGHT;

        // 若圆外线没有找到拐点
        if (Lpt0_circle_found == false && crpts0s_num > 0.15 / sample_dist)
        {
            // 圆环状态改为跑左环
            circle_type = CIRCLE_LEFT_RUNNING;
        }
    }
    else if (circle_type == CIRCLE_LEFT_RUNNING)
    {
        // 当前状态为跑左圆环

        // 循线状态改为循圆环右线
        track_type = TRACK_RIGHT;

        // 若右线存在右拐角,且右拐角距离小于25cm
        if (Lpt1_found_right == true)
        {
            if (Lpt1_rpts1s_id_right < 0.25 / sample_dist)
            {
                // 圆环状态改变为出右环
                circle_type = CIRCLE_LEFT_OUT;
            }
            else
                rptsc1_num = Lpt1_rpts1s_id_right + 2;
        }
    }
    else if (circle_type == CIRCLE_LEFT_OUT)
    {
        // 当前状态为出左环

        cipts1_num = sizeof(cipts1) / sizeof(cipts1[0]);
        // 反逆透视将右线拐点在原图坐标存入
        inv_Perspective(rpts1s[Lpt1_rpts1s_id_right][0], rpts1s[Lpt1_rpts1s_id_right][1], &Lpt1_c_x, &Lpt1_c_y);

        // 将拐点之前的坐标反逆透视进圆环右边线数组中
        for (int i = 0; i < Lpt1_rpts1s_id_right; i++)
        {
            inv_Perspective(rpts1s[i][0], rpts1s[i][1], &cipts1[i][0], &cipts1[i][1]);
            temp_line = i;
        }

        // 计算要补线的斜率
        int start_index = 0, end_index = temp_line;
        int start_x = cipts1[start_index][0] + 5, start_y = cipts1[start_index][1];
        int end_x = cipts1[end_index][0], end_y = cipts1[end_index][1];

        float k = 0.0f, b = 0.0f;
        k = (float) ((float)end_x  - (float)start_x) / (float) ((float)end_y - (float)start_y);
        b = (float) start_x - (float) start_y * k;

        end_y -= 10;

        // 通过该斜率找到前方赛道上的边线点
        for ( ; end_y > block_size; --end_y)
        {
            end_x = k * end_y + b;
            if (end_x <= block_size)
                break;

            if (AT_IMAGE(&img_raw, end_x, end_y) < thres)
                break;
        }
        ++end_y;
        end_x = k * end_y + b;

        // 补圆环右线从拐点沿切线方向补线至前方赛道
        cipts1_num = ImageAddingLine(cipts1, Lpt1_c_x, Lpt1_c_y, end_x, end_y, temp_line, cipts1_num);

        // 将补出的右线逆透视转换
        if (cipts1_num != 0)
        {
            for (int i = 0; i < cipts1_num; i++)
            {
                value = ImagePerspective(cipts1[i][0], cipts1[i][1], &crpts1[i][0], &crpts1[i][1]);
                if (value == 1)
                    break;
                crpts1_num = i;
            }
        }
        else
            crpts1_num = 0;

        // 对右边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts1, crpts1_num, crpts1b, (int) round(line_blur_kernel));
        crpts1b_num = crpts1_num;

        // 对右边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts1s_num = sizeof(crpts1s) / sizeof(crpts1s[0]);
        resample_points2(crpts1b, crpts1b_num, crpts1s, &crpts1s_num, sample_dist * pixel_per_meter);

        // 计算右中线
        track_rightline(crpts1s, crpts1s_num, crptsc1, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
        crptsc1_num = crpts1s_num;
        // 循右线
        track_type = TRACK_CIRCLE_RIGHT;

        // 右线无右拐点或者没有右线时
        if (Lpt1_rpts1s_id_right < 0.10 / sample_dist)// || Lpt1_found_right == false || ipts1_num == 0 /*|| Lpt1_rpts1s_id > 0.9 / sample_dist*/)
        {
            // 状态改为出左圆环
            circle_type = CIRCLE_LEFT_END;
        }
    }
    else if (circle_type == CIRCLE_LEFT_END)
    {
        // 当前状态为出左圆环
        cipts1_num = sizeof(cipts1) / sizeof(cipts1[0]);

        // 补线,从拐点开始补线至前方边线的点
        temp_line = ImageAddingLine_kb(cipts1, Lpt1_c_x, Lpt1_c_y, Lpt_k, Lpt_b, 0, cipts1_num);
        cx1 = cipts1[temp_line][0], cy1 = cipts1[temp_line][1];

        // 从前方边线开始爬圆环右线
        findline_righthand_adaptive(&img_raw, block_size, thres, cx1, cy1, cipts1, &cipts1_num, temp_line);

        // 将圆环右边线逆透视转换
        if (cipts1_num != 0)
        {
            for (int i = 0; i < cipts1_num; i++)
            {
                value = ImagePerspective(cipts1[i][0], cipts1[i][1], &crpts1[i][0], &crpts1[i][1]);
                if (value == 1)
                    break;
                crpts1_num = i;
            }
        }
        else
            crpts1_num = 0;

        // 对右边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts1, crpts1_num, crpts1b, (int) round(line_blur_kernel));
        crpts1b_num = crpts1_num;

        // 对右边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts1s_num = sizeof(crpts1s) / sizeof(crpts1s[0]);
        resample_points2(crpts1b, crpts1b_num, crpts1s, &crpts1s_num, sample_dist * pixel_per_meter);

        // 计算右边线
        track_rightline(crpts1s, crpts1s_num, crptsc1, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
        crptsc1_num = crpts1s_num;
        // 状态改为循右线
        track_type = TRACK_CIRCLE_RIGHT;

        if (is_straight1 == true)
        {
            // 圆环状态改为无圆环,循右线
            circle_type = CIRCLE_NONE;
            track_type = TRACK_RIGHT;
            crpts0s_num = crpts1s_num = 0;
        }
    }

    if(circle_type == CIRCLE_check_lost_line_again_RIGHT)
    {
        // 记录圆环内边线中最靠上的坐标值,为CIRCLE_IN爬线做起点
        int point_index = 0;
        for (int i = 1; i < ipts1_num; ++i)
        {
            if (ipts1[i][1] < ipts1[point_index][1])
                point_index = i;
        }
        cx1 = temp_1 = ipts1[point_index][0] - 15, cy1 = temp_2 = ipts1[point_index][1] - 10;

        track_type = TRACK_RIGHT;
        if (rpts1s_num > 0.10 / sample_dist && rpts1s_num < 0.20 / sample_dist)
        {
            // 改变状态为进入右圆环
            circle_type = CIRCLE_RIGHT_IN;
            track_type = TRACK_RIGHT;
        }
    }
    else if (circle_type == CIRCLE_RIGHT_IN)
    {
        // 当前圆环状态为进入右侧圆环
        cipts1_num = sizeof(cipts1) / sizeof(cipts1[0]);

        // 若圆环内边线足够长则再次查找最靠上点
        if (ipts1_num >= 0.06 / sample_dist && ipts1_num <= 0.30 / sample_dist)
        {
            int point_index = 0;
            for (int i = 1; i < ipts1_num; ++i)
            {
                if (ipts1[i][1] < ipts1[point_index][1])
                    point_index = i;
            }
            cx1 = temp_1 = ipts1[point_index][0] - 15, cy1 = temp_2 = ipts1[point_index][1] - 10;
        }
        else
        {
            // 不够长时使用前一次查找的坐标
            cx1 = temp_1;
            cy1 = temp_2;
        }
        // 向上找到圆环外边线
        for ( ; cy1 - 1 > block_size; cy1--)
        {
            if (AT_IMAGE(&img_raw, cx1, cy1 - 1) < thres)
                break;
        }

        // 爬圆环外线
        findline_righthand_adaptive(&img_raw, block_size, thres, cx1, cy1, cipts1, &cipts1_num, 0);
        // 对圆环外线进行进行逆透视变换,将爬到的在原图的坐标转换为在逆透视图上的坐标
        if (cipts1_num != 0)
        {
            for (int i = 0; i < cipts1_num; i++)
            {
                value = ImagePerspective(cipts1[i][0], cipts1[i][1], &crpts1[i][0], &crpts1[i][1]);
                if (value == 1)
                    break;
                crpts1_num = i;
            }
        }
        else
            crpts1_num = 0;

        // 对外边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts1, crpts1_num, crpts1b, (int) round(line_blur_kernel));
        crpts1b_num = crpts1_num;

        // 对外边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts1s_num = sizeof(crpts1s) / sizeof(crpts1s[0]);
        resample_points2(crpts1b, crpts1b_num, crpts1s, &crpts1s_num, sample_dist * pixel_per_meter);

        // 计算外边线的局部角变化率
        local_angle_points(crpts1s, crpts1s_num, crpts1a, (int) round(angle_dist / sample_dist));
        crpts1a_num = crpts1s_num;

        cipts0_num = sizeof(cipts0) / sizeof(cipts0[0]);
        // 查找外边线上拐角
        find_circle_corners(2, crpts1a, crpts1a_num, &Lpt1_circle_id);

        // 将拐点转换为在原图坐标存入左边线的圆环x,y
        inv_Perspective(crpts1s[Lpt1_circle_id][0], crpts1s[Lpt1_circle_id][1], &Lpt1_c_x, &Lpt1_c_y);

        // 补线,左边线起始点补线至左边线的圆环x,y,存入左线
        temp_line = ImageAddingLine(cipts0, ipts0[0][0], ipts0[0][1], Lpt1_c_x, Lpt1_c_y, 0, cipts0_num);

        // 对补出的线进行反逆透视转换进原图上的坐标
        for (int i = temp_line, j = Lpt1_circle_id;; i++, j--)
        {
            if (j > 0 && i < cipts0_num)
            {
                inv_Perspective(crpts1s[j][0], crpts1s[j][1], &cipts0[i][0], &cipts0[i][1]);
            }
            else
            {
                cipts0_num = i;
                break;
            }
        }

        // 将在原图上的补出的线转换在逆透视图像上
        if (cipts0_num != 0)
        {
            for (int i = 0; i < cipts0_num; i++)
            {
                value = ImagePerspective(cipts0[i][0], cipts0[i][1], &crpts0[i][0], &crpts0[i][1]);
                if (value == 1)
                    break;
                crpts0_num = i;
            }
        }
        else
            crpts0_num = 0;

        // 对左边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts0, crpts0_num, crpts0b, (int) round(line_blur_kernel));
        crpts0b_num = crpts0_num;

        // 对左边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts0s_num = sizeof(crpts0s) / sizeof(crpts0s[0]);
        resample_points2(crpts0b, crpts0b_num, crpts0s, &crpts0s_num, sample_dist * pixel_per_meter);

        // 计算中线
        track_leftline(crpts0s, crpts0s_num, crptsc0, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
        crptsc0_num = crpts0s_num;
        // 循左长线
        track_type = TRACK_CIRCLE_LEFT;

        // 若圆外线没有找到拐点
        if (Lpt1_circle_found == false && crpts1s_num > 0.15 / sample_dist)
        {
            // 圆环状态改为跑右环
            circle_type = CIRCLE_RIGHT_RUNNING;
        }
    }
    else if (circle_type == CIRCLE_RIGHT_RUNNING)
    {
        // 当前状态为跑右圆环

        // 循线状态改为循左线
        track_type = TRACK_LEFT;

        // 若左线存在左拐角,且左拐角距离小于25cm
        if (Lpt0_found_left == true)
        {
            if (Lpt0_rpts0s_id_left < 0.25 / sample_dist)
            {
                // 圆环状态改变为出右环
                circle_type = CIRCLE_RIGHT_OUT;
            }
            else
                rptsc0_num = Lpt0_rpts0s_id_left + 2;
        }
    }
    else if (circle_type == CIRCLE_RIGHT_OUT)
    {
        // 当前状态为出右环

        cipts0_num = sizeof(cipts0) / sizeof(cipts0[0]);
        // 反逆透视将左线拐点在原图坐标存入
        inv_Perspective(rpts0s[Lpt0_rpts0s_id_left][0], rpts0s[Lpt0_rpts0s_id_left][1], &Lpt1_c_x, &Lpt1_c_y);

        // 将拐点之前的坐标反逆透视进圆环左边线数组中
        for (int i = 0; i < Lpt0_rpts0s_id_left; i++)
        {
            inv_Perspective(rpts0s[i][0], rpts0s[i][1], &cipts0[i][0], &cipts0[i][1]);
            temp_line = i;
        }

        // 计算要补线的斜率
        int start_index = 0, end_index = temp_line;
        int start_x = cipts0[start_index][0] - 5, start_y = cipts0[start_index][1];
        int end_x = cipts0[end_index][0], end_y = cipts0[end_index][1];

        float k = 0.0f, b = 0.0f;
        k = (float) ((float)end_x  - (float)start_x) / (float) ((float)end_y - (float)start_y);
        b = (float) start_x - (float) start_y * k;
        end_y -= 10;

        // 通过该斜率找到前方赛道上的边线点
        for ( ; end_y > block_size; --end_y)
        {
            end_x = k * end_y + b;
            if (end_x <= block_size)
                break;
            if (AT_IMAGE(&img_raw, end_x, end_y) < thres)
                break;
        }
        ++end_y;
        end_x = k * end_y + b;

        // 补圆环左线从拐点沿切线方向补线至前方赛道
        cipts0_num = ImageAddingLine(cipts0, Lpt1_c_x, Lpt1_c_y, end_x, end_y, temp_line, cipts0_num);

        // 将补出的左线逆透视转换
        if (cipts0_num != 0)
        {
            for (int i = 0; i < cipts0_num; i++)
            {
                value = ImagePerspective(cipts0[i][0], cipts0[i][1], &crpts0[i][0], &crpts0[i][1]);
                if (value == 1)
                    break;
                crpts0_num = i;
            }
        }
        else
            crpts0_num = 0;

        // 对左边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts0, crpts0_num, crpts0b, (int) round(line_blur_kernel));
        crpts0b_num = crpts0_num;

        // 对左边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts0s_num = sizeof(crpts0s) / sizeof(crpts0s[0]);
        resample_points2(crpts0b, crpts0b_num, crpts0s, &crpts0s_num, sample_dist * pixel_per_meter);

        // 计算左中线
        track_leftline(crpts0s, crpts0s_num, crptsc0, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
        crptsc0_num = crpts0s_num;
        // 循左线
        track_type = TRACK_CIRCLE_LEFT;

        // 左线无左拐点或者没有左线时
        if (Lpt0_rpts0s_id_left < 0.10 / sample_dist)
        {
            // 状态改为出左圆环
            circle_type = CIRCLE_RIGHT_END;
        }
    }
    else if (circle_type == CIRCLE_RIGHT_END)
    {
        // 当前状态为出右圆环
        cipts0_num = sizeof(cipts0) / sizeof(cipts0[0]);

        // 补线,从拐点开始补线至前方边线的点
        temp_line = ImageAddingLine_kb(cipts0, Lpt1_c_x, Lpt1_c_y, Lpt_k, Lpt_b, 0, cipts0_num);
        cx0 = cipts0[temp_line][0], cy0 = cipts0[temp_line][1];

        // 爬圆环左线
        findline_lefthand_adaptive(&img_raw, block_size, thres, cx0, cy0, cipts0, &cipts0_num, temp_line);

        // 将圆环左边线逆透视转换
        if (cipts0_num != 0)
        {
            for (int i = 0; i < cipts0_num; i++)
            {
                value = ImagePerspective(cipts0[i][0], cipts0[i][1], &crpts0[i][0], &crpts0[i][1]);
                if (value == 1)
                    break;
                crpts0_num = i;
            }
        }
        else
            crpts0_num = 0;

        // 对左边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
        blur_points(crpts0, crpts0_num, crpts0b, (int) round(line_blur_kernel));
        crpts0b_num = crpts0_num;

        // 对左边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        crpts0s_num = sizeof(crpts0s) / sizeof(crpts0s[0]);
        resample_points2(crpts0b, crpts0b_num, crpts0s, &crpts0s_num, sample_dist * pixel_per_meter);

        // 计算左边线
        track_leftline(crpts0s, crpts0s_num, crptsc0, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
        crptsc0_num = crpts0s_num;
        // 状态改为循左线
        track_type = TRACK_CIRCLE_LEFT;

        if (is_straight0 == true)
        {
            // 圆环状态改为无圆环,循右线
            circle_type = CIRCLE_NONE;
            track_type = TRACK_LEFT;
            crpts0s_num = crpts1s_num = 0;
        }
    }
}

// 补线
int ImageAddingLine (int pts[][2], int startX, int startY, int endX, int endY, int num, int pts_num)
{
    int i = 0, j = 0;

    // 补线公式: x = ky + b
    float k = 0.0f, b = 0.0f;

    // 分别计算k,b
    k = (float) ((float) endX - (float) startX) / (float) ((float) endY - (float) startY);
    b = (float) startX - (float) startY * k;
    Lpt_k = k, Lpt_b = b;

    // 补线
    for (i = startY, j = num; i > endY; i--, j++)
    {
        if (j >= pts_num)
            break;
        pts[j][0] = (int) (k * i + b);
        pts[j][1] = (int) i;
    }

    // 返回最后一个元素下标
    return j - 1;
}

// 根据传入的斜率进行补线,补线至黑色跳变点
int ImageAddingLine_kb (int pts[][2], int startX, int startY, float k, float b, int num, int pts_num)
{
    int i = 0;
    int y = startY;
    int x = (int) (k * y + b);

    // 根据传入斜率补线至跳变点
    for (i = num; y > block_size && AT_IMAGE(&img_raw, x, y) >= thres; ++i)
    {
        if (i >= pts_num)
            break;
        if (x <= block_size)
            break;
        pts[i][0] = x;
        pts[i][1] = y;

        --y;
        x = (int) (k * y + b);
    }

    // 返回最后一个元素下标
    return i - 1;
}

// 进入圆环时,找圆环外边线角点
int find_circle_corners (uint8 mode, float pts[], int pts_num, int *id)
{
    int i = 0;
    Lpt0_circle_found = Lpt1_circle_found = false;
    switch (mode)
    {
    case 1 :
        // 左圆环外边线
        for (i = 0; i < pts_num; i++)
        {
            if (pts[i] < -70. / 180. * PI)
            {
                // 找到拐点记录
                Lpt0_circle_found = true;
                *id = i;
                break;
            }
        }
        break;
    case 2 :
        // 右圆环外边线
        for (i = 0; i < pts_num; i++)
        {
            if (pts[i] > 70. / 180. * PI)
            {
                // 找到拐点记录
                Lpt1_circle_found = true;
                *id = i;
                break;
            }
        }
        break;
    }
    return 0;
}

